from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import ThisLaunchFileDir, LaunchConfiguration
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    pkg_livox = os.path.join(
        get_package_share_directory('livox_ros_driver2'), 'launch_ROS2', 'msg_MID360_launch.py'
    )
    pkg_fastlio = os.path.join(
        get_package_share_directory('fast_lio'), 'launch', 'mapping.launch.py'
    )

    config_file_arg = DeclareLaunchArgument(
        'config_file',
        default_value='mid360.yaml',
        description='YAML config file for FAST_LIO'
    )
    config_file = LaunchConfiguration('config_file')

    rviz_cfg_arg = DeclareLaunchArgument(
        'rviz_cfg',
        default_value=os.path.join(
            get_package_share_directory('fast_lio'), 'rviz', 'fastlio.rviz'
        ),
        description='RViz configuration file for FAST_LIO'
    )
    rviz_cfg = LaunchConfiguration('rviz_cfg')

    return LaunchDescription([
        config_file_arg,
        rviz_cfg_arg,
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(pkg_livox)
        ),
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(pkg_fastlio),
            launch_arguments={'config_file': config_file, 'rviz_cfg': rviz_cfg}.items()
        ),
    ])